#include "sensors.h"

/*sensor_control_block g_scb;*/
volatile enum sensor current_sensor;
volatile uint8_t sensor_reading;
volatile uint8_t sample_cnt;
volatile static int16_t acc;

// Called by interrupt
// sample set function so that IR sensor has the maximum amount of time
// to adjust to current lght levels before returning a value
static int16_t get_IR_sample(){
  ADC10CTL0 |= ADC10ON | ADC10ENC | ADC10SC;
  while(ADC10CTL1 & ADC10BUSY);
  int16_t reading = 1024 - (0x03FF & ADC10MEM0);
  P2OUT ^= BIT0;
  return reading;
}

// Called by interrupt
// The only difference between this and the IR is that this method is
// set sample rather than sample set
static int16_t get_EF_sample(){
  P2OUT ^= BIT0;
  volatile int16_t spinner= 0;
  while(spinner++ < 500); //modify to give proper delay time after set
  ADC10CTL0 |= ADC10ON | ADC10ENC | ADC10SC;
  while(ADC10CTL1 & ADC10BUSY);
  return 0x03FF & ADC10MEM0;
}

//setup the pins so that infrared can be read
void initialize_infrared(){
    // Deselect Special functions
    P1SEL &= ~(BIT6);
    P2SEL &= ~(BIT0);
   
    // Set pins to output
    PJDIR |= BIT0 + BIT1;
    P1DIR |= BIT6;
    P2DIR |= BIT0;
}

//sets up the multiplexer to index into a sensor.
void select_sensor(sensor_sel_t sensor){
    uint8_t desired_j = sensor >> 1;
    uint8_t desired_p6 = (sensor & 1) << 6;
    PJOUT = (PJOUT &0xfc) | desired_j; // (clear current_setting | set new setting)
    P1OUT = (P1OUT & ~(0x01 << 6)) | desired_p6; // (clear current setting | set new setting)
    //{PJ.1, PJ.0, P1.6}
    /*
     * LED1 {0,0,0} Forward
     * LED2 {0,0,1} Reverse
     * LED3 {0,1,0} Right
     * LED4 {0,1,1} Left
     * LED5 {1,0,0} Down*/
}

// sets the state of whatever sensor is currently being multiplexed to
inline void set_sensor_state(state_t state){
    P2OUT = (P2OUT & ~BIT0) | state;   
}


// initialization of all sensor interrupts, none of them are enabled yet.
void initialize_sensors(void) {
    ////////////////////////////////////////
    // Initialize ADC10
    
    ADC10CTL0 |= ADC10SHT_0 | ADC10ON;
                //ADC10SHTx - what to set to? sample and hold time
                //ADC10ON - turn on (start) the ADC.  Seems like this should
                //only be set during actual sampling.
                //ADC10ENC - enable conversion.  What's the difference?  I
                //assume this simply takes a little more energy than turning
                //off
                //ADC10SC - start conversion.  Again, advantage/disadvantage
                //over ADC10ENC
    
    ADC10CTL1 |= ADC10SHP;
    ADC10CTL2 |= ADC10RES;
    ADC10MCTL0 |= ADC10INCH_0;

    P6SEL |= BIT0; // turn off digital input for P6.0
    //ADC10MCTL0
    
    ////////////////////////////////////////
    // Initialize Timer A1 for sensor readings
    P1SEL &= (~BIT6);
    P2SEL &= (~BIT0);

    PJDIR |= BIT0 + BIT1;
    P1DIR |= BIT6;
    P2DIR |= BIT0;

    PJOUT &= ~(BIT0 + BIT1);
    P1OUT &= ~BIT6;
    P2OUT &= ~BIT0;

    TA1CTL = TASSEL_2 | ID_0 | MC_1 | TACLR; //Read from SMCLK and
                                    //Count up to CCR0 NO TIMER A INTERRUPT
                                    //timer is EXPLICITLY initialized to off
                                    //turn on only for sampling
    TA1CCR0 = 1200;
    //TA1CCTL0 = CCIE;
    
    initialize_infrared();

    return;
}

// Gets a reading for a single sensor channel
// gets a reading from the type of sensor given the sensor's type, its
// mux value, its the number of samples to take, and the write address
// for the result.
static void get_reading(enum sensor sensor_type, sensor_sel_t sensor,
                        uint8_t num_samples, int16_t *dest) {
    current_sensor = sensor_type;
    acc = 0;
    sample_cnt = 0;
    select_sensor(sensor);
    set_sensor_state(LOW);
    ENABLE_SENSOR_INTERRUPTS;
    TA1_ON;
    //get enough samples, this spins until an appropriate number of samples
    //is taken.
    while (sample_cnt < num_samples) {
        // LPM?
        volatile uint8_t vu = 0;
        vu++;
    }
    DISABLE_SENSOR_INTERRUPTS;
    TA1_OFF;
    set_sensor_state(LOW);
    *dest = acc;
    return;
}
    

//void gather_sensor_data (sensor_data_t *sdp) {
void gather_sensor_data (int16_t *sdp) {
    // Set header
    sdp[0] = (int16_t)SENSOR_DATA;
    
    //IR Sensors get data!
    get_reading(IR, FORWARD, 64, sdp+1);
    get_reading(IR, REVERSE, 64, sdp+2);
    get_reading(IR, LEFT,    64, sdp+3);
    get_reading(IR, RIGHT,   64, sdp+4);
    get_reading(IR, DOWN,    64, sdp+5);
    // get_reading(IR, FORWARD, 30, &(sdp->ir_front));
    // get_reading(IR, REVERSE, 30, &(sdp->ir_reverse));
    // get_reading(IR, LEFT,    30, &(sdp->ir_left));
    // get_reading(IR, RIGHT,   30, &(sdp->ir_right));
    // get_reading(IR, DOWN,    30, &(sdp->ir_down));    

#ifdef USING_EF_SENSORS
    //EFIELD_SENSORS
    get_reading(EF, 6, 32, &(sdp->ef_0));    
    get_reading(EF, 7, 32, &(sdp->ef_1));    
#endif

    set_sensor_state(LOW);

#ifdef USING_MAG_SENSOR
    sdp->mag = 1;
#endif

    return;
}



// Timer A1 interrupt service routine
#ifdef __GNUC__
INTERRUPT(TIMER1_A0_VECTOR, Timer1_A0_ISR) {
#else
#pragma vector=TIMER1_A0_VECTOR
__interrupt void Timer1_A0_ISR(void) {
#endif
    // Simply reset the timer and start over
    int16_t reading;
    //executes sensor read based on the type of the current_sensor
    switch (current_sensor){
      case IR:
        reading = get_IR_sample();
        //cancels out noise by subtracting readings taken when the light is off
        if (P2OUT & BIT0) {
          acc -= reading;
        } else {
          acc += reading;
        }
        break;
      case EF:
        reading = get_EF_sample();
        //cancels out noise by subtracting readings when EF is not active
        if (P2OUT & BIT0) {
          acc -= reading;
        } else {
          acc += reading;
        }
        break;
    }
    sample_cnt ++;
    //exit low power mode

    //__bic_SR_register_on_exit(LPM0_bits);
}

